/*
 * Copyright (C) 2020 spreadtrum.com
 */

#include <linux/types.h>
#include <malloc.h>
#include <common.h>
#include <mmc.h>



#include <sprd_rpmb.h>
#include <trusty/trusty_dev.h>
#include <trusty/trusty_ipc.h>

#include <maintdata.h>



#define MAINT_BL_PORT    "com.android.trusty.maint.bl"
#define MAINT_DATA_MAX_SIZE 1024
#define MAINT_RPMB_DATA_SIZE 256
#define MAINT_RPMB_PAC_SIZE 512
#define MAINT_BL_SESSION_MAGIC 0x53535343 


struct maint_bl_msg {
    uint32_t cmd;
    uint32_t op_id;
    uint32_t size;
    int32_t  result;
    uint16_t blk_ind;
    uint32_t wr_count;
    uint8_t  data_wr[MAINT_RPMB_DATA_SIZE];
    uint8_t  pac[MAINT_RPMB_PAC_SIZE];
};


enum maint_bl_cmd {
    MAINT_BL_REQ_SHIFT = 1,
    MAINT_BL_RESP_BIT  = 1,

    MAINT_BL_RESP_MSG_ERR   = MAINT_BL_RESP_BIT,

    MAINT_BL_DATA_RD    = 1 << MAINT_BL_REQ_SHIFT,
    MAINT_BL_DATA_WR	= 2 << MAINT_BL_REQ_SHIFT,

};


enum maint_bl_err {
    MAINT_BL_NO_ERROR          = 0,
    MAINT_BL_ERR_GENERIC       = 1,
    MAINT_BL_ERR_NOT_VALID     = 2,
    MAINT_BL_ERR_UNIMPLEMENTED = 3,
    MAINT_BL_ERR_ACCESS        = 4,
    MAINT_BL_ERR_NOT_FOUND     = 5,
    MAINT_BL_ERR_EXIST         = 6,
};



static struct trusty_ipc_dev* maint_ipc_dev;
static struct trusty_dev maint_tdev; /* There should only be one trusty device */
static struct trusty_ipc_chan maint_chan;



int get_maintdata_blk_ind(void)
{
#ifdef CONFIG_SPRD_RPMB
    u64 rpmb_size = 0;

    rpmb_size = emmc_get_capacity(PARTITION_RPMB);

    if (0 > rpmb_size ){
        printf("%s: get rpmb block count error! return code %d \n", __func__, rpmb_size);
        return -1;
    }
    return (rpmb_size / MAINT_RPMB_DATA_SIZE) - 12;

#endif
}


int read_maint_data_from_rpmb(uint8_t *buf, int len)
{
    uint8_t data_rd[MAINT_DATA_MAX_SIZE];
    uint16_t block_ind, block_count;
    int ret = 0, i = 0;

    if (NULL == buf) {
        printf("%s: buffer is NULL\n", __func__);
        return -1;
    }

    if ((0 >= len) || (MAINT_DATA_MAX_SIZE < len)) {
        printf("%s: invalid buffer length %d\n", __func__, len);
        return -1;
    }

    block_ind = get_maintdata_blk_ind();


    memset(data_rd, 0x0, sizeof(data_rd));
    block_count = sizeof(data_rd) / MAINT_RPMB_DATA_SIZE;
    for (i = 0; i < block_count; i++) {
        ret = rpmb_blk_read(data_rd + (i * MAINT_RPMB_DATA_SIZE), block_ind + i, 1);
        if(ret < 0) {
            printf("%s: maint data read %d fail! ret %d \n", __func__, block_ind + i, ret);
            return ret;
        }
    }

    printf("%s: maint data read (%d %d) successful \n", __func__, block_ind, block_ind + i);


    memcpy((void *)buf, data_rd, len);

    return 0;
}

int write_maint_data_to_rpmb(uint8_t *buf, int len)
{
    uint8_t data_rd[MAINT_DATA_MAX_SIZE];
    uint16_t block_ind, block_count;
    struct maint_bl_msg msg;
    int ret = -1, i = 0, j = 0;
    struct trusty_ipc_iovec req_iov = {
            .base = &msg, 
            .len = sizeof(msg)
    };

    if (NULL == buf) {
        printf("%s: buffer is NULL\n", __func__);
        return -1;
    }

    if ((0 >= len) || (MAINT_DATA_MAX_SIZE < len)) {
        printf("%s: invalid buffer length %d\n", __func__, len);
        return -1;
    }

    block_ind = get_maintdata_blk_ind();

    memset(data_rd, 0x0, sizeof(data_rd));
    block_count = sizeof(data_rd) / MAINT_RPMB_DATA_SIZE;
    for (j = 0; j < block_count; j++) {

        ret = rpmb_blk_read(data_rd + (j * MAINT_RPMB_DATA_SIZE), block_ind + j, 1);
        if(ret < 0) {
            printf("%s: maint data read %d fail! ret %d \n", __func__, block_ind + j, ret);
            return ret;
        }

        printf("%s: maint data read (%d %d) successful \n", __func__, block_ind, block_ind +j);
    }

    for (i = 0; i < len; i++) {
        if (data_rd[i] == 0x0) {
            data_rd[i] = buf[i];
        }
    }


    /* init Trusty device */
    ret = trusty_dev_init(&maint_tdev, NULL);
    if (ret != 0) {
        printf("Initializing maint Trusty device failed (%d)\n", ret);
        return ret;
    }

    /* create Trusty IPC device */
    ret = trusty_ipc_dev_create(&maint_ipc_dev, &maint_tdev, PAGE_SIZE);
    if (ret != 0) {
        printf("Initializing maint Trusty IPC device failed (%d)\n", ret);
        goto err_dev;
    }

    trusty_ipc_chan_init(&maint_chan, maint_ipc_dev);


    /* connect to km service and wait for connect to complete */
    ret = trusty_ipc_connect(&maint_chan, MAINT_BL_PORT, true);
    if (ret < 0) {
        printf("failed (%d) to connect to '%s'\n", ret, MAINT_BL_PORT);
        goto err_ipc_dev;
    }

    for (j = 0; j < block_count; j++) {

        memcpy((void *)msg.data_wr, data_rd + (j * MAINT_RPMB_DATA_SIZE), MAINT_RPMB_DATA_SIZE);
        msg.cmd = MAINT_BL_DATA_WR;
        msg.blk_ind = block_ind + j;
        msg.wr_count = rpmb_read_writecount();
        memset(msg.pac, 0x0, sizeof(msg.pac));

        printf("%s: maint read rpmb wirte count %d successful \n", __func__, msg.wr_count);

        ret = trusty_ipc_send(&maint_chan, &req_iov, 1, true);
        if (ret != 0) {
            printf("send maint message failed (%d)\n", ret);
            goto err_chan;
        }

        ret = trusty_ipc_recv(&maint_chan, &req_iov, 1, true);
        if (ret < 0) {
            printf("failed (%d) to recv maint response\n", ret);
            goto err_chan;
        }
        if (ret < sizeof(struct maint_bl_msg)) {
            printf("invalid maint response size (%d)\n", ret);
            ret = -1;
            goto err_chan;
        }
        if (msg.cmd != (MAINT_BL_DATA_WR | MAINT_BL_RESP_BIT)) {
            printf("invalid maint response cmd 0x%x\n", msg.cmd);
            ret = -1;
            goto err_chan;
        }
        if (msg.result != MAINT_BL_NO_ERROR) {
            printf("invalid maint response result 0x%x\n", msg.result);
            ret = -1;
            goto err_chan;
        }

        ret = rpmb_write_pac(msg.pac, block_ind + j);
        if(ret < 0) {
            printf("%s: maint data write %d fail! ret %d \n", __func__, block_ind + j, ret);
            return ret;
        }

        printf("%s: maint data write %d successful \n", __func__, block_ind +j);
    }

    (void)trusty_ipc_close(&maint_chan);

    (void)trusty_ipc_dev_shutdown(maint_ipc_dev);

    (void)trusty_dev_shutdown(&maint_tdev);


    return 0;

err_chan:
     (void)trusty_ipc_close(&maint_chan); 
err_ipc_dev:
     (void)trusty_ipc_dev_shutdown(maint_ipc_dev);
err_dev:
    (void)trusty_dev_shutdown(&maint_tdev);
    return ret;
}

